#include <FastSerial.h>
#include "ACCEL.h"
#include <avr/eeprom.h>
#include <iic.h>

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif

//#define ACC_ORIENTATION(x, y, z) {raw_data[0]=-y; raw_data[1]=-z; raw_data[2]=-x;}

#define BMA180_ADDRESS 0x80

accel_setting_t accel_setting_eeprom EEMEM;

const int16_t Accelerometer::G = 512;

void Accelerometer::init()
{
    delay(10);
    //default range 2G: 1G = 4096 unit.
    i2c_writeReg(BMA180_ADDRESS,0x0D,1<<4); // register: ctrl_reg0  -- value: set bit ee_w to 1 to enable writing
    delay(5);
    uint8_t control = i2c_readReg(BMA180_ADDRESS, 0x20);
    control = control & 0x0F; // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 10Hz (bits value = 0000xxxx)

    i2c_writeReg(BMA180_ADDRESS, 0x20, control);
    delay(5);
    control = i2c_readReg(BMA180_ADDRESS, 0x30);
    control = control & 0xFC;
    control = control | 0x02;
    i2c_writeReg(BMA180_ADDRESS, 0x30, control);
    delay(5);

    vector.zero();
    setting.init();
    calibrating = -1;
    //start_smooth = true;
}

inline void Accelerometer::save_setting()
{
    eeprom_write_block(&setting, &accel_setting_eeprom, sizeof(setting));
}

void Accelerometer::load_setting()
{
	  if (eeprom_read_byte(&accel_setting_eeprom.mark) == 'A')
	  	 eeprom_read_block(&setting, &accel_setting_eeprom, sizeof(setting));
}

void Accelerometer::calibrate()
{
    calibrating = 512;
    for (uint8_t axis = 0; axis < 3; axis++)
    {
        sum.sum_32[axis] = 0;
        setting.zero[axis] = 0;
        data[axis] = 0;
//        vector[axis] = 0;
    }
    vector.zero();
}

inline void Accelerometer::read(int16_t out[])
{
    i2c_getSixRawADC(BMA180_ADDRESS, 0x02);

    out[0] = -((rawADC[2] | (rawADC[3]<<8)) >> 5);
    out[1] = (rawADC[4] | (rawADC[5]<<8)) >> 5;
    out[2] = (rawADC[0] | (rawADC[1]<<8)) >> 5;
}

void Accelerometer::update()
{
    uint8_t axis;
    int16_t raw[3];

    read(raw);

    static uint32_t time;
    if (millis() > time)
    {
        //Serial.printf("ACC %5i %5i %5i\n", raw[0], raw[1], raw[2]);
        time = millis() + 500;
    }

    if (calibrating > 0)
    {
        for (axis = 0; axis < 3; axis++)
            sum.sum_32[axis] += raw[axis];

        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
        if (calibrating == 1)
        {
            setting.zero[0] = sum.sum_32[0] >> 9;
            setting.zero[1] = sum.sum_32[1] >> 9;
            setting.zero[2] = (sum.sum_32[2] >> 9) - G;

            //accTrim[ROLL] = 0;
            //accTrim[PITCH] = 0;
            //writeParams();//ACCУ׼ֵд��EEPROM
            save_setting();
            //Serial.printf("Acc calibrated. x=%i y=%i z=%i\n", zero[0], zero[1], zero[2]);
            //start_smooth = true;
            calibrating--;
        }
        calibrating--;
    }

    if (calibrating < 0)
    {
        for (axis = 0; axis < 3; axis++)
        {
            data[axis] = raw[axis] - setting.zero[axis];
            sum.sum_16[axis] = data[axis] << 4;
//            vector[axis] = data[axis];
        }
        //start_smooth = false;
        calibrating = 0;
    }
    else if (calibrating == 0)
    {
        for (axis = 0; axis < 3; axis++)
        {
            sum.sum_16[axis] += raw[axis] - data[axis] - setting.zero[axis];
            data[axis] = sum.sum_16[axis] >> 4;
            //vector[axis] = data[axis];
        }
    }

		vector.x = data[0];
		vector.y = data[1];
		vector.z = data[2];
}
